/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include <GL/glut.h>
#include <GL/gl.h>
#include "FleetFormationWXGLFrame.hpp"

#define MINSCALE 1000
#define MAXSCALE 40

BEGIN_EVENT_TABLE(CFleetFormationWXGLFrame,wxGLCanvas)
EVT_PAINT(CFleetFormationWXGLFrame::OnPaint)
EVT_SIZE(CFleetFormationWXGLFrame::OnSize)
EVT_ERASE_BACKGROUND(CFleetFormationWXGLFrame::OnErase)

EVT_LEFT_DOWN(CFleetFormationWXGLFrame::OnLeftDown)
EVT_LEFT_UP(CFleetFormationWXGLFrame::OnLeftUp)
EVT_LEFT_DCLICK(CFleetFormationWXGLFrame::OnDoubleClick)
EVT_RIGHT_DOWN(CFleetFormationWXGLFrame::OnRightDown)
EVT_RIGHT_UP(CFleetFormationWXGLFrame::OnRightUp)
EVT_MOTION(CFleetFormationWXGLFrame::OnMotion)
  EVT_MOUSEWHEEL(CFleetFormationWXGLFrame::OnMouseWheel)
  EVT_LEAVE_WINDOW(CFleetFormationWXGLFrame::OnLeaveWindow)
    

  EVT_CHAR(CFleetFormationWXGLFrame::OnChar)
  //EVT_CONTEXT_MENU(CFleetFormationWXGLFrame::OnContextMenu)
END_EVENT_TABLE()

int CFleetFormationWXGLFrame::attribList[3]={WX_GL_DOUBLEBUFFER,WX_GL_RGBA,0};


CFleetFormationWXGLFrame::CFleetFormationWXGLFrame(wxWindow* parent, FleetFormationGUIMainFrame* mainFrame)
 :wxGLCanvas(parent, wxID_ANY, attribList),
  CSync(1,1),
  mainFrame(mainFrame),
  GLContext(this),
  iView(),
  followSelected(false),
  iRobotMap(NULL),
  iMasterRobotIter(),
  iSelectedRobotIterator(),
  iAttachedRobotIterator(),
  iIsRobotAttached(false),
  // iMasterHeading(0.0),
  grid(true),
  gridZ(-1.0),
  gridSize(1),
  iShowRealPose(false),
  iScreenLocked(false)
 
{
  
  iView.Scale=100; //How many pixels is one meter
  iView.OriginX=0;
  iView.OriginY=0;
  iView.parkAreaSize = 100;
  this->SetSize(800,600);
  RunThread();
}

CFleetFormationWXGLFrame::~CFleetFormationWXGLFrame()
{
  SetRequestTermination();
}




/****************** event handlers **************************/


void CFleetFormationWXGLFrame::OnErase(wxEraseEvent &e)
{
  /*do nothing*/
}

void CFleetFormationWXGLFrame::OnPaint(wxPaintEvent &e)
{

  Draw();
}

void CFleetFormationWXGLFrame::OnSize(wxSizeEvent &e)
{
  dPrint(1,"OnSize... ");
  iView.Width=e.GetSize().GetWidth();
  iView.Height=e.GetSize().GetHeight();
}

void CFleetFormationWXGLFrame::OnLeftDown(wxMouseEvent &e)
{

  //Do nothing is screen is locked
  if(iScreenLocked) return;


  if(iRobotMap ==NULL){
    return;
  }
  long mousex,mousey;
  e.GetPosition(&mousex,&mousey);
  dPrint(1,"On left down...%ld;%ld ",mousex,mousey);
  
  std::map<std::string,SRobotIcon *>::iterator iter = iRobotMap->begin();
  while(iter != iRobotMap->end()){
    if(iter->second->IsInsideRobot(mousex,mousey)){
      

      iIsRobotAttached = true;          
      iAttachedRobotIterator = iter;
      iSelectedRobotIterator = iter;
      
    
      break;
    }
    iter++;
  }
}

void CFleetFormationWXGLFrame::OnLeftUp(wxMouseEvent &e)
{
  iIsRobotAttached = false;
    
}

void CFleetFormationWXGLFrame::OnDoubleClick(wxMouseEvent &e)
{
 
}

void CFleetFormationWXGLFrame::OnRightDown(wxMouseEvent &e)
{


 //Do nothing is screen is locked
  if(iScreenLocked) return;

  if(iRobotMap ==NULL){
    return;
  }
  long mousex,mousey;
  e.GetPosition(&mousex,&mousey);
  dPrint(1,"On right down...%ld;%ld ",mousex,mousey);

  std::map<std::string,SRobotIcon *>::iterator iter = iRobotMap->begin();
  while(iter != iRobotMap->end()){
    if(iter->second->IsInsideRobot(mousex,mousey)){
      
      
      if(iter != iMasterRobotIter){
        iMasterRobotIter =iter;
        
        
      }else{
        iter = iRobotMap->end();
      }
    break;      
    }


  iter++;
  }


  
}

void CFleetFormationWXGLFrame::OnRightUp(wxMouseEvent &e)
{
  
}

void CFleetFormationWXGLFrame::OnMotion(wxMouseEvent &e)
{
  //Do nothing is screen is locked
  if(iScreenLocked) return;

  long mousex,mousey;
  e.GetPosition(&mousex,&mousey);
  if(iIsRobotAttached){
    iAttachedRobotIterator->second->SetPosition(mousex,mousey);
    
  }
}

void CFleetFormationWXGLFrame::OnMouseWheel(wxMouseEvent &e)
{
    int dir=e.GetWheelRotation();
    int delta=e.GetWheelDelta();
    int newScale = iView.Scale;

    if (delta!=0){
      newScale=iView.Scale+iView.Scale*dir/delta*0.10;
    }
 
    //Looks a little bit strange, but it goes this way. Scale means how many pixels
    //is one meter
    if(newScale < MAXSCALE){
      iView.Scale= MAXSCALE;
    }else if(newScale > MINSCALE){
      iView.Scale = MINSCALE;
    }else{
      iView.Scale = newScale;
    }
}

void CFleetFormationWXGLFrame::OnLeaveWindow(wxMouseEvent &e)
{

}

void CFleetFormationWXGLFrame::OnChar(wxKeyEvent &e)
{
}


/************************************************************/
void CFleetFormationWXGLFrame::SetView()
{

    
  /* iView.Left=iView.OriginX-this->GetSize().GetWidth()/(float)iView.Scale/2.0;
  iView.Right=iView.OriginX+this->GetSize().GetWidth()/(float)iView.Scale/2.0;

  iView.Bottom=iView.OriginY-this->GetSize().GetHeight()/(float)iView.Scale/2.0;
  iView.Top=iView.OriginY+this->GetSize().GetHeight()/(float)iView.Scale/2.0; //100 for "park area"
  */
  iView.Left = 0;
  iView.Right = this->GetSize().GetWidth();
  iView.Top = 0;
  iView.Bottom = this->GetSize().GetHeight();
  SetCurrent(GLContext);
  
  glClearColor(CLEAR_COLOR,0);

  glEnable(GL_DEPTH_TEST);
  
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glViewport(0,0,GetSize().GetWidth(),GetSize().GetHeight());
   glOrtho(iView.Left,iView.Right,iView.Bottom,iView.Top,-1000,1000);

  double pm[16];
  glGetDoublev(GL_PROJECTION_MATRIX,pm);
  //SetProjectionMatrix(pm);
  
}

void CFleetFormationWXGLFrame::Draw(){
  //  dPrint(1,"Drawing! Sixe w:%d h:%d",GetSize().GetWidth(),GetSize().GetHeight());
  int width = 0;
  int height = 0;
  this->GetSize(&width,&height);


  //dPrint(1,"Size of frame w:%d h:%d",width,height);
  SetCurrent(GLContext);
  SetView();

  glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
  
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  
  double mm[16];
  glGetDoublev(GL_MODELVIEW_MATRIX,mm);
  //SetModelviewMatrix(mm);


    DrawGrid();
  DrawRobots();

  glFlush();
  SwapBuffers();
}
//******************************************************************************
void CFleetFormationWXGLFrame::DrawGrid()
{
  float x, y;
  glLineWidth(1);
  glBegin(GL_LINES);
    //draw grid
  glColor3f(GRID_COLOR);
  //  dPrint(1,"top %f, bottom %f",iView.Top, iView.Bottom);
  /*for (x= iView.Left-fmod(iView.Left,gridSize); x<=iView.Right;x+=gridSize)
  {
    glVertex3f(x,iView.Top-iView.parkAreaSize/iView.Scale,gridZ);
    glVertex3f(x,iView.Bottom,gridZ);
  }
  for ( y=iView.Bottom-fmod(iView.Bottom,gridSize); y<=iView.Top-iView.parkAreaSize/iView.Scale; y+=gridSize)
  {
    glVertex3f(iView.Left,y,gridZ);
    glVertex3f(iView.Right,y,gridZ);

    }*/

  for (x= 0; x<=this->GetSize().GetWidth();x+=gridSize*iView.Scale)
  {
    glVertex3f(x,iView.parkAreaSize ,gridZ);
    glVertex3f(x,this->GetSize().GetHeight(),gridZ);
  }
  for ( y=iView.parkAreaSize ;y <= this->GetSize().GetHeight(); y+=gridSize*iView.Scale)
  {
    glVertex3f(0,y,gridZ);
    glVertex3f(this->GetSize().GetWidth(),y,gridZ);

  }


    //draw coordinate axes
  /*  glColor3f(AXIS_COLOR);
  glVertex3f(0,iView.Top,gridZ+0.1);
  glVertex3f(0,iView.Bottom,gridZ+0.1);
  glVertex3f(iView.Left,0,gridZ+0.1);
  glVertex3f(iView.Right,0,gridZ+0.1);*/
  glEnd();
}
//********************************************************************************
int CFleetFormationWXGLFrame::ThreadFunction(const int aThreadNumber)
{ 
  ownSleep_ms(1000); //this prevents a segfault :P
  //dPrint(1,"thread running");
  while (!IsRequestTermination())
  {
      //refresh graphics
    wxPaintEvent e;
    AddPendingEvent(e);
    
    ownSleep_ms(50);
  }
  return 0;

}

const TViewParams& CFleetFormationWXGLFrame::GetViewParams()
{

}


void CFleetFormationWXGLFrame::DrawRobots(){

  if(iRobotMap ==NULL){
    return;
  }
  std::map<std::string,SRobotIcon *>::iterator iter = iRobotMap->begin();
  int masterX=-1;
  int masterY = -1;
  float masterRealX= 0, masterRealY = 0;
  float x1,y1,x2,y2,x3,y3;
  //Lock();
  if(iMasterRobotIter != iRobotMap->end()){
    masterX = iMasterRobotIter->second->positionX;
    masterY = iMasterRobotIter->second->positionY;
    MaCI::Position::CPositionData pData;
    //Get master heading...
    iMasterRobotIter->second->iPositionClient-> GetPositionEvent(pData, 
                                                                 NULL,
                                                                 0);
    
    const MaCI::Position::TPose2D *pose= pData.GetPose2D();
    if(pose == NULL){
      dPrint(1,"Pose ==NULL");
      return;
    }

    masterRealX = pose->x;
    masterRealY = pose->y;

    gim::CSync::Lock();
    
    std::map<std::string,SRobotIcon *>::iterator iter = iRobotMap->begin();
    while(iter != iRobotMap->end()){
      
      iter->second->heading = pose->a;
      iter++;
    }      
    gim::CSync::Unlock();
  
  



  }

 //Draw robots
  while(iter != iRobotMap->end()){
      glBegin(GL_LINES);
    if(iter == iMasterRobotIter){
      glColor3f(255,255,0);     
    }else{    

      glColor3f(255,255,255);
    }
      iter->second->GetTriangleCorners(x1,y1,x2,y2,x3,y3);
      dPrint(10,"Drawingrobot to %f %f %f %f %f %f, position is %d %d, heading %f",x1,y1,x2,y2,x3,y3,iter->second->positionX,iter->second->positionY,iter->second->heading);
      
      glVertex3f(x1,y1,gridZ);    
      glVertex3f(x2,y2,gridZ);   
      
      glVertex3f(x2,y2,gridZ);    
      glVertex3f(x3,y3,gridZ);    
      
      glVertex3f(x3,y3,gridZ);    
      glVertex3f(x1,y1,gridZ);    

      glVertex3f(x1,y1,gridZ);
      glVertex3f((x2+x3)/2.0,(y2+y3)/2,gridZ);
      glEnd();
    


    if(masterX != -1 && masterY !=-1 && iter->second->positionY > 100 &&  iter->second->positionX < this->GetSize().GetWidth()){
      glBegin(GL_LINES);
      glColor3f(0,125,0);
      glVertex3f(masterX,masterY,gridZ);    
      glVertex3f(iter->second->positionX,iter->second->positionY,gridZ);    
      glEnd();
      if(iter != iMasterRobotIter){
        float xDiff = -(float)(masterX -iter->second->positionX)/(float)iView.Scale;
        float yDiff = (float)(masterY -iter->second->positionY)/(float)iView.Scale;
        char buffer[256];
        sprintf(buffer,"[%.2f m; %.2f m]",xDiff,yDiff);
        
        std::string differenceText(buffer);
        int textlength = differenceText.size();
        glColor3f(255,255,255);
        drawText(iter->second->positionX-textlength*3,iter->second->positionY+35,0,GLUT_BITMAP_HELVETICA_10, differenceText.c_str());
      }
    }
    
    int textlength = iter->first.size();
    //Draw name of the machine
    glColor3f(255,255,255);
    drawText(iter->second->positionX-textlength*3,iter->second->positionY+20,0,GLUT_BITMAP_HELVETICA_10, iter->first.c_str());

    iter++;
   
  }


  //Draw slave's real position 
  if(iMasterRobotIter != iRobotMap->end() &&iShowRealPose){
      iter = iRobotMap->begin();
      while(iter != iRobotMap->end()){
        if(iter != iMasterRobotIter){
          MaCI::Position::CPositionData pData;
          //Get master heading...
          iter->second->iPositionClient-> GetPositionEvent(pData, 
                                                           NULL,
                                                           0);
          const MaCI::Position::TPose2D *pose= pData.GetPose2D();
          if(pose == NULL){
            dPrint(1,"Pose ==NULL");
            return;
          }
          float diffX = masterRealX - pose->x;
          float diffY = masterRealY - pose->y;
          SRobotIcon rIcon(masterX - diffX*iView.Scale, masterY + diffY*iView.Scale);
          rIcon.heading = pose->a;
         
          
          rIcon.GetTriangleCorners(x1,y1,x2,y2,x3,y3);
          dPrint(100,"Drawingrobot real pose to %f %f %f %f %f %f, position is %d %d, heading %f, diffX %f , diffY %f, master (%f,%f)",x1,y1,x2,y2,x3,y3,pose->x,pose->y,iter->second->heading,diffX,diffY, masterRealX, masterRealY);


          glBegin( GL_LINE_STRIP);
          glColor3f(255,0,0);         


          glVertex3f(x1,y1,gridZ);    
          glVertex3f(x2,y2,gridZ);   
          
          glVertex3f(x2,y2,gridZ);    
          glVertex3f(x3,y3,gridZ);    
          
          glVertex3f(x3,y3,gridZ);    
          glVertex3f(x1,y1,gridZ);    
          
          glVertex3f(x1,y1,gridZ);
          glVertex3f((x2+x3)/2.0,(y2+y3)/2,gridZ);
          
          
          glEnd();
          //Draw name of the machine
          int textlength = iter->first.size();

          drawText(rIcon.positionX-textlength*3,rIcon.positionY+20,0,GLUT_BITMAP_HELVETICA_10, iter->first.c_str());
          
        // GL_LINE_STRIP
        }

        iter++;
      } 
    



  }
  //Unlock();
}

void CFleetFormationWXGLFrame::SetRobots(std::map<std::string, SRobotIcon*> *aRobotMap)
{
  iRobotMap = aRobotMap;
  iMasterRobotIter = iRobotMap->end();
  std::map<std::string, SRobotIcon*>::iterator iter =iRobotMap->begin();
  
}

 
std::map<std::string, SRobotIcon*>::iterator CFleetFormationWXGLFrame::GetMasterRobot(){

  return iMasterRobotIter; 
}

void CFleetFormationWXGLFrame::GetSlaveRobots(std::vector<SSlaveMachine> *aSlaveVector,
                                              bool aRelativeToMasterHeading){
  if(iRobotMap == NULL){
    return;
  }


  if(iMasterRobotIter == iRobotMap->end()){
    dPrint(1,"No master robot set. Can't go to formation");
    return;
  }
  int masterX=iMasterRobotIter->second->positionX;
  int masterY =iMasterRobotIter->second->positionY;
  float masterHeading = iMasterRobotIter->second->heading;
  dPrint(1,"Searching the slaves. Number of robots %d",iRobotMap->size());
  std::map<std::string,SRobotIcon *>::iterator iter = iRobotMap->begin();
  while(iter != iRobotMap->end()){
    
    if(iter->second->positionX >= GetSize().GetWidth()||
       iter->second->positionX < 0 ||
       iter->second->positionY > GetSize().GetHeight() ||
       iter->second->positionY <= 100){
      dPrint(1,"Robot not in grid. skipping it (%d,%d) ", iter->second->positionX,iter->second->positionY);
      iter++;
      continue;
    }
    
    if(iter == iMasterRobotIter){
      iter++;
      continue;
    }

    float xDiff = -(float)(masterX -iter->second->positionX)/(float)iView.Scale;
    float yDiff = (float)(masterY -iter->second->positionY)/(float)iView.Scale;
    if(iter->second->iSpeedCtrlClient != NULL &&iter->second->iPositionClient !=NULL){
      MaCI::Position::TPose2D slavePose;
      float relativeX = -((float)masterX -(float)iter->second->positionX)/(float)iView.Scale;
      //Because Y is flipped
      float relativeY = ((float)masterY - (float)iter->second->positionY)/(float)iView.Scale;

      dPrint(1," masterPose %d %d %f, relative is %f %f", masterX,masterY,masterHeading,relativeX,relativeY); 
      ///Slave's relative positions
      
      if(aRelativeToMasterHeading){
        slavePose.x = relativeX *cos(-masterHeading) - relativeY*sin(-masterHeading);
        slavePose.y = (relativeX *sin(-masterHeading) + relativeY*cos(-masterHeading));
        //slavePose.x = -slavePose.x;
        //slavePose.y = -slavePose.y;
        dPrint(1," masterPose %d %d %f, relative is %f %f", masterX,masterY,masterHeading,relativeX,relativeY); 
        dPrint(1,"slavePose %d %d, Setting slaves to %f %f",iter->second->positionX,iter->second->positionY,slavePose.x,slavePose.y);
        slavePose.a= 0;
      }else{
        slavePose.x = relativeX;
        slavePose.y = relativeY;
        slavePose.a = 0.0;
      }
      dPrint(1,"Adding a slave");
      aSlaveVector->push_back(SSlaveMachine(iter->second->iPositionClient, iter->second->iSpeedCtrlClient,slavePose));
      
    }
  
    iter++;    
  }
  
  
}

//function for drawing text
void CFleetFormationWXGLFrame::drawText(float x, float y, float z, void *font, const char *string)
{
  const char *c;
  glRasterPos3f(x,y,z);
  for (c=string; *c != '\0'; c++)
    {
      glutBitmapCharacter(font, *c);
    }
}

//******************************************************************************
void CFleetFormationWXGLFrame::ShowRealPose(bool aState)
{

  iShowRealPose = aState;

}
//******************************************************************************

void CFleetFormationWXGLFrame::LockGLScreen()
{
  dPrint(1,"locking screen");
  iScreenLocked = true;

}
//******************************************************************************

void CFleetFormationWXGLFrame::UnlockGLScreen()
{
  dPrint(1,"Unlocking screen");
  iScreenLocked = false;
}
//******************************************************************************

void CFleetFormationWXGLFrame::DrawRelativeToMasterHeading(bool aState)
{
  iDrawRelativeToMasterHeading = aState;
}
//******************************************************************************
